#define LOG_TAG "ins.tag"
#include "ins_det.h"
#include "elog.h"

#include "../drivers/adc.h"
#include "device.h"

#include "utils.h"
#include "relay_ctrl.h"

#include "cmsis_os2.h"

#include "macro_math.h"

extern rt_adc_device_t adc;
static int ins_state;

// --- Globals ---
double v_iso_P;        // Isolation voltage Vp
double v_iso_N;        // Isolation voltage Vn
double v_DC_P, v_DC_N; // DC BUS voltage during Vp measurement and Vn measurement

// According ADC voltages
double v_adc_iso_pos;          // voltage at the ADC input during Vp measurement
double v_adc_iso_neg;          // voltage at the ADC input during Vn measurement
double v_adc_DC_P, v_adc_DC_N; // voltage at the ADC for DC BUS voltage measurement during Vp measurement and Vn measurement

// Circuit parameters and coefficients for the switched in resistor divider
double r_ps = R_PS_DFLT;
double r_ns = R_NS_DFLT;
double r_s = R_S_DFLT;
double v_ref = V_REF_DFLT;
double k_adc = K_ADC_DFLT;
double k_diff2sinISO = k_DIFF2SINISO_DFLT;

// Circuit parameters and coefficients for Bus Monitoring
double r_ps_DC = R_PS_DFLT_DC;
double r_s_DC = R_S_DFLT_DC;
double v_ref_DC = V_REF_DFLT_DC;
double k_batt = K_BATT_DFLT;
double k_diff2sinBATT = k_DIFF2SINBATT_DFLT;

// Calculated insulation resistances
double r_iso_p, r_iso_n;
double t_acq = T_ACQ_DFLT; // Acquisition time

// 转成国标定义
static uint32_t prv = 0, nrv = 0;

// --- Function prototypes ---
void calculate_riso(void);                 // Function to calculate insulation resistances out of the measured results.
int measure_voltages_VN_V_DC_N(void);      // Function to measure the insulation voltage Vp and DC Bus voltage when the upper switch is closed
int measure_voltages_VP_VDC_P(void);       // Function to measure the insulation voltage Vn and DC Bus voltage when the lower switch is closed

int measure_voltages_VP_VDC_P(void)
{
    int ret = 0;
    v_adc_iso_pos = 0;
    static uint32_t adcRaw_VP = 0, adcRaw_DC_P = 0;
    static int ch_i = 0;

    if (ch_i >= 10)
    {
        ch_i = 0;
        v_adc_iso_pos = (double)adcRaw_VP * ADC_REF / 4096 / 10;
        log_d("v_adc_iso_pos %.2f", v_adc_iso_pos);
        v_iso_P = ((r_ps + r_s) / r_s) * ((v_adc_iso_pos - v_ref) / (k_diff2sinISO * k_adc));
        log_d("v_iso_P %.2f", v_iso_P);
        v_adc_DC_P = (double)adcRaw_DC_P * ADC_REF / 4096 / 10;
        v_DC_P = ((r_ps_DC + r_s_DC) / r_s_DC) * ((v_adc_DC_P - v_ref_DC) / (k_batt * k_diff2sinBATT));
        log_d("v_DC_P %.2f", v_DC_P);
        adcRaw_VP = 0, adcRaw_DC_P = 0;
        ret = pdTRUE;
    }
    else
    {
        ch_i++;
        rt_adc_enable(adc, INS_SP_CHN);
        adcRaw_VP += rt_adc_read(adc, INS_SP_CHN);
        rt_adc_enable(adc, INS_SN_CHN);
        adcRaw_DC_P += rt_adc_read(adc, INS_SN_CHN);
    }
    return ret;
}

int measure_voltages_VN_V_DC_N(void)
{
    int ret = 0;
    v_adc_iso_neg = 0;
    static uint32_t adcRaw_VN = 0, adcRaw_DC_N = 0;
    static int ch_i = 0;

    if (ch_i >= 10)
    {
        ch_i = 0;
        v_adc_iso_neg = (double)adcRaw_VN * ADC_REF / 4096 / 10;
        log_d("v_adc_iso_neg %.2f", v_adc_iso_neg);
        v_iso_N = ((r_ps + r_s) / r_s) * ((v_adc_iso_neg - v_ref) / (k_diff2sinISO * k_adc));
        log_d("v_iso_N %.2f", v_iso_N);
        v_adc_DC_N = (double)adcRaw_DC_N * ADC_REF / 4096 / 10;
        v_DC_N = ((r_ps_DC + r_s_DC) / r_s_DC) * ((v_adc_DC_N - v_ref_DC) / (k_batt * k_diff2sinBATT));
        log_d("v_DC_N %.2f", v_DC_N);
        adcRaw_VN = 0, adcRaw_DC_N = 0;
        ret = pdTRUE;
    }
    else
    {
        ch_i++;
        rt_adc_enable(adc, INS_SP_CHN);
        adcRaw_VN += rt_adc_read(adc, INS_SP_CHN);
        rt_adc_enable(adc, INS_SN_CHN);
        adcRaw_DC_N += rt_adc_read(adc, INS_SN_CHN);
    }
    return ret;
}

static int measure_voltages_DC(void)
{
    int ret = 0;
    static uint32_t adcRaw_DC_N = 0;
    static int ch_i = 0;

    if (ch_i >= 10)
    {
        ch_i = 0;
        v_adc_DC_N = (double)adcRaw_DC_N * ADC_REF / 4096 / 10;
        v_DC_N = ((r_ps_DC + r_s_DC) / r_s_DC) * ((v_adc_DC_N - v_ref_DC) / (k_batt * k_diff2sinBATT));
        log_d("v_DC_N %.2f", v_DC_N);
        adcRaw_DC_N = 0;
        ret = pdTRUE;
    }
    else
    {
        ch_i++;
        rt_adc_enable(adc, INS_SN_CHN);
        adcRaw_DC_N += rt_adc_read(adc, INS_SN_CHN);
    }
    return ret;
}

/**
 * @brief                   高压开关控制
 * @param  relay            继电器索引
 * @param  state            继电器状态  0：断开 1：闭合
 */
static void actuate_relay(const uint16_t relay, const uint16_t state)
{
    switch (relay)
    {
    case RELAY_POS:
        relay_ctrl(RELAY_K5, state);
        break;
    case RELAY_NEG:
        relay_ctrl(RELAY_K6, state);
        break;
    default:
        break;
    }
}

/**
 * @brief   计算绝缘电阻
 */
void calculate_riso(void)
{
    r_iso_n = ((r_s + r_ns) * (r_s + r_ps) * \
    (v_DC_N * v_DC_P + v_DC_P * v_iso_N - v_DC_N * v_iso_P)) / \
    (v_iso_P * (r_ns * v_iso_N - r_ps * v_iso_N + v_DC_N * r_s + v_DC_N * r_ns));
    r_iso_p = -((r_s + r_ns) * (r_s + r_ps) * \
    (v_DC_N * v_DC_P + v_DC_P * v_iso_N - v_DC_N * v_iso_P)) / \
    (v_iso_N * (r_ns * v_iso_P - r_ps * v_iso_P + v_DC_P * r_s + v_DC_P * r_ps));
    nrv = (uint32_t)(r_iso_n / v_DC_N);
    prv = (uint32_t)(r_iso_p / v_DC_P);
    log_d("rn %.2f, rp %.2f, nrv %d, prv %d", r_iso_n, r_iso_p, nrv, prv);
}

/**
 * @brief 
 * @return int 
 */
int insulation_detection_fsm(void)
{
    int ret = 0;
    static int state = INS_IDLE_STATE;

    switch(state)
    {
        case INS_IDLE_STATE:
            ins_state = 0;
            /* 使能绝缘检测 */
            measure_voltages_DC();
            if (event_get_bit(ENABLE_INS_EVT))
            {
                state = INS_POS_ON_NEG_OFF_STATE;
            }
            break;
        case INS_POS_ON_NEG_OFF_STATE:
            actuate_relay(RELAY_POS, ON);                       /* K5闭合K6断开 */
            actuate_relay(RELAY_NEG, OFF);
            state = INS_GET_VP_STATE;
            break;
        case INS_GET_VP_STATE:
            if (measure_voltages_VP_VDC_P() >= pdTRUE)          /* 检测P点电压核直流母线电压 */
                state = INS_POS_OFF_NEG_ON_STATE;
            break;
        case INS_POS_OFF_NEG_ON_STATE:
            actuate_relay(RELAY_POS, OFF);                      /* K2闭合K1断开 */
            actuate_relay(RELAY_NEG, ON);
            state = INS_GET_VN_STATE;
            break;
        case INS_GET_VN_STATE:
            if (measure_voltages_VN_V_DC_N() >= pdTRUE)         /* 检测N点电压和直流母线电压 */
                state = INS_GET_RISO_STATE;
            break;
        case INS_GET_RISO_STATE:
            calculate_riso();                                   /* 计算阻值 */
            state = INS_END_STATE;
            break;
        case INS_END_STATE:
            actuate_relay(RELAY_POS, OFF);
            actuate_relay(RELAY_NEG, OFF);
            state = INS_IDLE_STATE;
            if (min(prv, nrv) >= min(prv, nrv))
                ins_state = 1;                                  /* 绝缘检测成功 */
            else
                ins_state = 0;
            break;
        default:
            break;
    }
    return ret;
}

/**
 * @brief               Get the dc bus voltage object
 * @return uint32_t     voltage（+mv）
 */
uint32_t get_dc_bus_voltage(void)
{
    uint32_t ret = 0;

    ret = (uint32_t)(v_DC_N * 1000);       /* 转成+mv */
    return ret;
}

/**
 * @brief Get the ins state object
 * @return int 
 */
int get_ins_state(void)
{
    int ret = 0;

    ret = ins_state;
    return ret;
}

